#include "hmc5883l.h"
#include <string.h>

// 灵敏度查找表 (来自 HMC5883L 数据手册)
static const float hmc5883l_scale_table[] = {
    1370.0f,  // 0x00: ±0.88 Ga
    1090.0f,  // 0x01: ±1.3 Ga
    820.0f,   // 0x02: ±1.9 Ga
    660.0f,   // 0x03: ±2.5 Ga
    440.0f,   // 0x04: ±4.0 Ga
    390.0f,   // 0x05: ±4.7 Ga
    330.0f,   // 0x06: ±5.6 Ga
    230.0f    // 0x07: ±8.1 Ga
};

HMC5883L_Status HMC5883L_Init(HMC5883L* dev, Wire* wire, uint8_t addr) {
    if (!dev || !wire) return HMC5883L_ERROR;

    dev->wire = wire;
    dev->addr = addr;
    dev->mode = HMC5883L_MODE_IDLE;
    dev->scale = 1090.0f; // 默认值

    // 检查设备是否存在 (读取 ID 寄存器)
    uint8_t id[3];
    if (HMC5883L_GetID(dev, id) != HMC5883L_OK) {
        return HMC5883L_ERROR_DEV_NOT_FOUND;
    }

    // 可选：验证 ID 值（HMC5883L 的 ID 应为 'H', '4', '3'）
    // 但某些模块是 HMC5883（非L），可能无ID，所以可跳过严格检查

    // 默认配置
    return HMC5883L_Config(dev, HMC5883L_DEFAULT_SAMPLES,
                           HMC5883L_DEFAULT_RANGE,
                           HMC5883L_DEFAULT_RATE);
}

HMC5883L_Status HMC5883L_Config(HMC5883L* dev, uint8_t samples, uint8_t range, uint8_t rate) {
    if (!dev) return HMC5883L_ERROR;

    uint8_t config_a = (samples << 5) | ((rate & 0x07) << 2);

    uint8_t config_b = range;  // 直接写入

    if (HMC5883L_WriteReg(dev, HMC5883L_REG_CONFIG_A, config_a) != HMC5883L_OK) {
        return HMC5883L_ERROR_COMM;
    }
    if (HMC5883L_WriteReg(dev, HMC5883L_REG_CONFIG_B, config_b) != HMC5883L_OK) {
        return HMC5883L_ERROR_COMM;
    }

    // 增益索引：range 的高 3 位
    uint8_t gain = (range >> 5) & 0x07;
    if (gain < 8) {
        dev->scale = hmc5883l_scale_table[gain];
    } else {
        dev->scale = 1090.0f;
    }

    return HMC5883L_OK;
}

HMC5883L_Status HMC5883L_SetMode(HMC5883L* dev, uint8_t mode) {
    if (HMC5883L_WriteReg(dev, HMC5883L_REG_MODE, mode) != HMC5883L_OK) {
        return HMC5883L_ERROR_COMM;
    }
    dev->mode = mode;
    return HMC5883L_OK;
}

HMC5883L_Status HMC5883L_ReadRaw(HMC5883L* dev, HMC5883L_MagData* data) {
    uint8_t buf[6];
    if (HMC5883L_ReadRegs(dev, HMC5883L_REG_DATA_X_H, buf, 6) != HMC5883L_OK) {
        return HMC5883L_ERROR_COMM;
    }

    // 正确解析：X, Z, Y 顺序
    data->x = (int16_t)(buf[0] << 8 | buf[1]);  // X 轴
    data->z = (int16_t)(buf[2] << 8 | buf[3]);  // Z 轴
    data->y = (int16_t)(buf[4] << 8 | buf[5]);  // Y 轴

    return HMC5883L_OK;
}

HMC5883L_Status HMC5883L_ReadScaled(HMC5883L* dev, float* x, float* y, float* z) {
    HMC5883L_MagData raw;
    if (HMC5883L_ReadRaw(dev, &raw) != HMC5883L_OK) {
        return HMC5883L_ERROR_COMM;
    }

    *x = (float)raw.x / dev->scale;
    *y = (float)raw.y / dev->scale;
    *z = (float)raw.z / dev->scale;

    return HMC5883L_OK;
}

HMC5883L_Status HMC5883L_GetID(HMC5883L* dev, uint8_t* id) {
    // HMC5883L 有 ID 寄存器 0x10, 0x11, 0x12，但某些模块可能不支持
    // 这里尝试读取，失败也不报错
    if (HMC5883L_ReadRegs(dev, 0x10, id, 3) != HMC5883L_OK) {
        return HMC5883L_ERROR_COMM;
    }
    return HMC5883L_OK;
}

// 内部写寄存器
HMC5883L_Status HMC5883L_WriteReg(HMC5883L* dev, uint8_t reg, uint8_t value) {
    if (Wire_writeReg(dev->wire, dev->addr, reg, value) != 0) {
        return HMC5883L_ERROR_COMM;
    }
    return HMC5883L_OK;
}

// 内部读多个寄存器
HMC5883L_Status HMC5883L_ReadRegs(HMC5883L* dev, uint8_t reg, uint8_t* buf, uint8_t len) {
    if (Wire_readRegs(dev->wire, dev->addr, reg, buf, len) != 0) {
        return HMC5883L_ERROR_COMM;
    }
    return HMC5883L_OK;
}
